#include "map_system.h"

#include "map_grid.h"

MapSystem::MapSystem(RegionSystemType system_type, uint32_t height,
                     uint32_t width)
    : system_type_(system_type),
      min_grid_index_(0),
      max_grid_index_(0),
      g_entity_id(0),
      height_(height),
      width_(width) {}

MapSystem::~MapSystem() {}

void MapSystem::Init() {
  auto size = height_ * width_;
  grids_.reserve(size);

  for (uint32_t i = 0; i < size; i++) {
    grids_.push_back(MapGrid(i, kMapGridStatusTypeFree));
  }
}

std::string MapSystem::Print() {
  std::string print;
  for (uint32_t i = 0; i < grids_.size(); i++) {
    if (grids_[i].GetStatus() == kMapGridStatusObstacle) {
      print += "*";
    } else {
      print += "#";
    }

    if ((i + 1) % (width_) == 0) {
      print += "\n";
    }
  }
  return print;
}

uint32_t MapSystem::ToMapIndex(uint32_t x_index, uint32_t y_index) const {
  assert(x_index < width_ && y_index < height_);
  return y_index * width_ + x_index;
}

uint32_t MapSystem::ToMapIndex(const Vector2D& pos) const {
  if (!IsInMap(pos)) {
    return kInvalidGridIndex;
  }
  return pos.GridY() * GetWidth() + pos.GridX();
}

bool MapSystem::MapIndexToPos(uint32_t index, Vector2D& pos) const {
  assert(index < height_ * width_);
  uint32_t x_index = index % width_;
  uint32_t y_index = index / width_;
  pos.SetX(x_index * kGridSize);
  pos.SetY(y_index * kGridSize);
}

bool MapSystem::CheckStatus(const Vector2D& pos,
                            EMapGridStatusType check_type) const {
  return CheckStatus(pos.GridX(), pos.GridY(), check_type);
}

bool MapSystem::CheckStatus(uint32_t index,
                            EMapGridStatusType check_type) const {
  if (index >= grids_.size()) {
    return kMapGridStatusObstacle & check_type;
  }
  return (grids_[index].GetStatus() & check_type) != 0;
}

bool MapSystem::CheckStatus(uint32_t x_index, uint32_t y_index,
                            EMapGridStatusType check_type) const {
  auto index = y_index * GetWidth() + x_index;
  return CheckStatus(index, check_type);
}

void MapSystem::OnEntityIntoMap(EntityPtr entity) {
  assert(entity);
  auto grid = GetGrid(entity->GetPosition());
  if (!grid) {
    return;
  }

  uint32_t x = entity->GetPosition().GridX();
  uint32_t y = entity->GetPosition().GridY();

  for (uint32_t xi = x; xi <= x + 1; xi++) {
    for (uint32_t yi = y; yi <= y + 1; yi++) {
      auto new_gird = GetGrid(xi, yi);
      // std::cout << "xi:" << xi << " yi:" << yi << std::endl;
      // std::cout << "index: " << new_gird->GetIndex() << " id:" << entity->GetEntityId() <<  std::endl;
      new_gird->AddEntity(entity);
    }
  }
}

// grid->AddEntity(entity);

// uint32_t x = entity->GetPosition().GridX();
// uint32_t y = entity->GetPosition().GridY();

// for (uint32_t xi = x; xi <= x + 1; xi++) {
//   for (uint32_t yi = y; yi <= y + 1; yi++) {
//       auto gird = GetGrid(xi, yi);
//               grid->AddEntity(entity);
//   }
// }
// system("clear");
// std::cout << ToString() << std::endl;

void MapSystem::OnEntityLeaveMap(EntityPtr entity) {
  assert(entity);

  auto grid = GetGrid(entity->GetPosition());
  assert(grid);

  grid->RemoveEntity(entity->GetEntityId());
  // system("clear");
  // std::cout << ToString() << std::endl;
}

void MapSystem::OnEntityChangePos(EntityPtr entity, const Vector2D& old_pos,
                                  const Vector2D& new_pos) {
  auto old_grid = GetGrid(old_pos);
  assert(old_grid);

  auto new_grid = GetGrid(new_pos);
  assert(new_grid);

  auto& scale = entity->GetScale();

  if (new_grid != old_grid) {
    old_grid->RemoveEntity(entity->GetEntityId());
    new_grid->AddEntity(entity);
  }

  // system("clear");

  // std::cout << "cur_pos:" << old_pos.X().GetValue() << " "
  //           << old_pos.Y().GetValue() << std::endl;
  // std::cout << "target_pos:" << new_pos.X().GetValue() << " "
  //           << new_pos.Y().GetValue() << std::endl;
  // std::cout << ToString() << std::endl;
}

MapGrid* MapSystem::GetGrid(const Vector2D& pos) {
  if (!IsInMap(pos)) {
    return nullptr;
  }

  return GetGrid(pos.GridX(), pos.GridY());
}

bool MapSystem::IsInMap(const Vector2D& pos) const {
  if (pos.X().ToGrid() >= GetWidth() || pos.X().ToGrid() < 0 ||
      pos.Y().ToGrid() >= GetHeight() || pos.Y().ToGrid() < 0) {
    return false;
  }
  return true;
}

MapGrid* MapSystem::GetGrid(uint32_t grid_index) {
  assert(grid_index < GetWidth() * GetHeight());
  assert(grids_.size() > grid_index);
  return &grids_[grid_index];
}

MapGrid* MapSystem::GetGrid(uint32_t x_index, uint32_t y_index) {
  uint32_t index = GetGridIndex(x_index, y_index);
  assert(index != kInvalidGridIndex);
  assert(grids_.size() > index);
  return &grids_[index];
}

uint32_t MapSystem::GetGridIndex(uint32_t x_index, uint32_t y_index) const {
  return ToMapIndex(x_index, y_index);
}

uint32_t MapSystem::GetGridIndex(const Vector2D& pos) const {
  if (!IsInMap(pos)) {
    return kInvalidGridIndex;
  }

  return GetGridIndex(pos.GridX(), pos.GridY());
}

std::string MapSystem::ToString() {
  std::string result;
  for (uint32_t i = 0; i < grids_.size(); i++) {
    if ((i + 1) % GetHeight() == 0) {
      result += "\n";
    }

    if (grids_[i].GetEntities().empty() ||
        grids_[i].CheckStatus(kMapGridStatusObstacle)) {
      result += " ";
    } else {
      result += "(";
      for (const auto& entity : grids_[i].GetEntities()) {
        result += std::to_string(entity->GetPosition().GridX());
        result += ",";
        result += std::to_string(entity->GetPosition().GridY());
      }
      result += ")";
    }
  }
  return result;
}

bool MapSystem::LineInterGrid(const Vector2D& pos_0, const Vector2D& pos_1,
                              std::list<uint32_t>& grid_indexs) {
  return LineInterGridZ(pos_0.X(), pos_0.Y(), pos_1.X(), pos_1.Y(),
                        grid_indexs);
}

bool MapSystem::LineInterGridZ(ZNumber x0, ZNumber y0, ZNumber x1, ZNumber y1,
                               std::list<uint32_t>& grid_indexs) {
  bool steep = (y1 - y0).Abs() > (x1 - x0).Abs();
  if (steep) {
    std::swap(x0, y0);
    std::swap(x1, y1);
  }

  bool swap_start = false;
  if (x0 > x1) {
    std::swap(x0, x1);
    std::swap(y0, y1);
    swap_start = true;
  }

  ZNumber grid_size(kGridSize);
  if (y0.ToGrid() == y1.ToGrid())  // ƽ��
  {
    for (ZNumber x = x0; x < x1.Ceil(grid_size);
         x = (x + grid_size).Floor(grid_size)) {
      AddInterGridZ(grid_indexs, x, y0, steep, swap_start);
    }
    return true;
  }

  ZNumber deltax = x1 - x0;
  ZNumber deltay = y1 - y0;

  ZNumber step_y = 0x7fffffff;
  if (deltax != 0) {
    step_y = deltay / deltax;
  }

  ZNumber y = 0;
  ZNumber pre_y = y0;
  ZNumber pre_x = x0;

  // std::cout << "pre_x:" << pre_x.GetValue() << std::endl;
  // std::cout << "pre_y:" << pre_y.GetValue() << std::endl;
  // std::cout << "step_y:" << step_y.GetValue() << std::endl;
  // std::cout << "x1:" << x1.GetValue() << std::endl;
  // std::cout << "y1:" << y1.GetValue() << std::endl;

  for (ZNumber x = x0; x <= x1.Ceil(grid_size);
       x = (x + grid_size).Floor(grid_size)) {
    y = y0 + (x - x0) * step_y;
    if (y.ToGrid() != pre_y.ToGrid()) {
      AddInterGridZ(grid_indexs, pre_x, y, steep, swap_start);
    }
    AddInterGridZ(grid_indexs, x, y, steep, swap_start);
    pre_x = x;
    pre_y = y;
  }

  return true;
}

void MapSystem::AddInterGridZ(std::list<uint32_t>& grid_indexs, ZNumber x,
                              ZNumber y, bool steep, bool swap_start) {
  uint32_t index = 0;
  if (steep) {
    index = GetGridIndex(y.ToGrid(), x.ToGrid());
  } else {
    index = GetGridIndex(x.ToGrid(), y.ToGrid());
  }

  if (swap_start) {
    grid_indexs.push_front(index);
  } else {
    grid_indexs.push_back(index);
  }
}

bool MapSystem::MapIndexToCenterPos(uint32_t x_index, uint32_t y_index,
                                    Vector2D& pos) const {
  assert(x_index < GetWidth() && y_index < GetHeight());
  ZNumber offset(kGridSize);
  // offset /= 2;
  pos.SetX(offset + static_cast<int32_t>(x_index * kGridSize));
  pos.SetY(offset + static_cast<int32_t>(y_index * kGridSize));
  return true;
}

bool MapSystem::IsRelationEntity(const Vector2D& start_pos,
                                 const Vector2D& target_pos,
                                 const Vector2D& center_pos, const ZNumber& cl,
                                 Vector2D& cvt) {
  auto v = target_pos - start_pos;
  if (v.LengthSquare() == 0) {
    return false;
  }

  auto cv = center_pos - start_pos;

  auto dir = v.Dot(cv);
  if (dir < 0)  // ??????
  {
    // ��??
    return false;
  }

  auto clls = cl * cl;
  auto tv = center_pos - target_pos;
  if (cv.LengthSquare() <= clls ||
      tv.LengthSquare() <= clls)  // ??????????,??????
  {
    return false;
  }

  auto vt = cv - v * cv.Dot(v) / v.LengthSquare();
  if (vt.LengthSquare() >= clls) {
    return false;
  }

  if (dir == 0) {
    return false;
  }

  dir = (-cv).Dot(-tv);
  if (dir > 0) {
    return false;
  }

  cvt = cv - vt;

  return true;
}

ECaculatePathResult MapSystem::CaculateMoveCriclePath(
    const Vector2D& start_pos, const Vector2D& target_pos,
    const Vector2D& center_pos, const ZNumber& cl, std::list<Vector2D>& paths) {
  if (cl == 0) {
    return kCaculatePathResultNone;
  }

  auto v = target_pos - start_pos;
  auto vls = v.LengthSquare();
  if (vls == 0) {
    return kCaculatePathResultNone;
  }

  auto cv = center_pos - start_pos;
  auto cvl = cv.Length();

  auto dir = v.Dot(cv);
  if (dir < 0)  // ??????
  {
    // ��??
    return kCaculatePathResultNone;
  }

  auto tv = target_pos - center_pos;
  auto tvl = tv.Length();
  if (tvl == 0) {
    return kCaculatePathResultNone;
  }
  auto tvls = tvl * tvl;

  if (cvl <= cl && tvl <= cl)  // ???????,??????
  {
    return kCaculatePathResultNone;
  } else if (tvl <= cl) {
    auto pv = cv.Normal() * cl;
    auto temp = center_pos - pv;
    paths.push_back(start_pos);
    paths.push_back(temp);
    return kCaculatePathResultPopTarget;
  }

  auto cross = cv * v;

  Vector2D inter_pos_1;
  Vector2D inter_pos_2;

  if (cross == 0)  // ????????
  {
    std::cout << "cross" << std::endl;
    auto base_v = cv.Normal() * cl;
    Matrix2 ts(0, -1, 1, 0);
    inter_pos_1 = center_pos - base_v;

    ZNumber cosv = cl / tvl;
    auto lq = (tvls - cl * cl).Sqrt();
    ZNumber sinv = lq / tvl;

    Matrix2 mt(cosv, -sinv, sinv, cosv);
    auto pv = (tv * mt).Normal() * cl;
    inter_pos_2 = center_pos + pv;
  } else {
    auto v = target_pos - start_pos;
    auto vls = v.LengthSquare();
    auto cv = center_pos - start_pos;
    auto cvl = cv.Length();

    auto tv = target_pos - center_pos;
    auto tvl = tv.Length();
    auto tvls = tvl * tvl;

    auto clls = cl * cl;
    auto temp = v * cv.Dot(v) / vls;
    auto p = cv - v * cv.Dot(v) / vls;
    auto pls = p.LengthSquare();

    if (pls >= clls)  // ��??
    {
      return kCaculatePathResultNone;
    }
    auto vl = (clls - pls).Sqrt();
    auto vs = v.Normal() * vl;
    auto iv = -vs - p;
    inter_pos_1 = center_pos + iv;
    ZNumber cosv = cl / tvl;
    auto lq = (tvls - cl * cl).Sqrt();
    ZNumber sinv = lq / tvl;

    Matrix2 mt(cosv, -sinv, sinv, cosv);
    auto pv = (tv * mt).Normal() * cl;
    auto dir = pv.Dot(-p);
    if (dir < 0) {
      Matrix2 mt(cosv, sinv, -sinv, cosv);
      pv = (tv * mt).Normal() * cl;
    }
    inter_pos_2 = center_pos + pv;
  }

  MakePath(inter_pos_1, inter_pos_2, center_pos, paths);
  paths.push_front(inter_pos_1);
  paths.push_back(inter_pos_2);
  return kCaculatePathResultMoveToTarget;
}
const double kMoveDir = 20.0;

void MapSystem::MakePath(const Vector2D& pos_1, const Vector2D& pos_2,
                         const Vector2D& center_pos,
                         std::list<Vector2D>& paths) {
  auto v1 = center_pos - pos_1;
  auto v2 = center_pos - pos_2;
  auto v3 = pos_2 - pos_1;

  if (v2.Length() == 0 || v1.Length() == 0) {
    return;
  }

  ZNumber result = v2.Dot(v1) / (v2.Length() * v1.Length());
  double arv = std::acos(result.ToFloat()) * 180.0 / M_PI;
  int32_t add_count = static_cast<int32_t>(arv / kMoveDir);
  if (add_count > 1) {
    auto dir = kMoveDir;
    auto dir2 = v1 * v3;
    if (dir2 < 0) {
      dir = -dir;
    }

    ZNumber sinv = std::sin(dir / 180.0 * M_PI);
    ZNumber cosv = std::cos(dir / 180.0 * M_PI);
    Matrix2 mt(cosv, -sinv, sinv, cosv);
    Vector2D temp = v1;
    for (int32_t i = 0; i < add_count; ++i) {
      temp = temp * mt;
      auto new_p = center_pos - temp;
      paths.push_back(new_p);
    }
  }
}
